#include "bsp_motor.h"

Motor_t Motor1, Motor2;

extern TIM_HandleTypeDef htim3;
void Motor_Init(void)
{
    Motor1.left_pin = MOTOR1_LEFT_PIN;
    Motor1.left_pin_port = MOTOR1_LEFT_PIN_PORT;
    Motor1.right_pin = MOTOR1_RIGHT_PIN;
    Motor1.right_pin_port = MOTOR1_RIGHT_PIN_PORT;

    Motor2.left_pin = MOTOR2_LEFT_PIN;
    Motor2.left_pin_port = MOTOR2_LEFT_PIN_PORT;
    Motor2.right_pin = MOTOR2_RIGHT_PIN;
    Motor2.right_pin_port = MOTOR2_RIGHT_PIN_PORT;
    
    Motor_Sleep(&Motor1,&Motor2);
}

void Motor_Sleep(Motor_t *M1, Motor_t *M2)
{
//    HAL_GPIO_WritePin(M1->left_pin_port, M1->left_pin, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(M1->right_pin_port, M1->right_pin, GPIO_PIN_RESET);

//     HAL_GPIO_WritePin(M2->left_pin_port, M2->left_pin, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(M2->right_pin_port, M2->right_pin, GPIO_PIN_RESET);
	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,0);	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,0);		
	
	
}

void Motor_Ahead(Motor_t *M1, Motor_t *M2)
{
//    HAL_GPIO_WritePin(M1->left_pin_port, M1->left_pin, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(M1->right_pin_port, M1->right_pin, GPIO_PIN_SET);

//     HAL_GPIO_WritePin(M2->left_pin_port, M2->left_pin, GPIO_PIN_SET);
//    HAL_GPIO_WritePin(M2->right_pin_port, M2->right_pin, GPIO_PIN_RESET);
	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,300);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,0);	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,300);	
}

void Motor_Stop(Motor_t *M1, Motor_t *M2)
{
//    HAL_GPIO_WritePin(M1->left_pin_port, M1->left_pin, GPIO_PIN_SET);
//    HAL_GPIO_WritePin(M1->right_pin_port, M1->right_pin, GPIO_PIN_SET);

//     HAL_GPIO_WritePin(M2->left_pin_port, M2->left_pin, GPIO_PIN_SET);
//    HAL_GPIO_WritePin(M2->right_pin_port, M2->right_pin, GPIO_PIN_SET);
	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,0);	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,0);
}

void Motor_Back(Motor_t *M1, Motor_t *M2)
{
//    HAL_GPIO_WritePin(M1->left_pin_port, M1->left_pin, GPIO_PIN_SET);
//    HAL_GPIO_WritePin(M1->right_pin_port, M1->right_pin, GPIO_PIN_RESET);

//     HAL_GPIO_WritePin(M2->left_pin_port, M2->left_pin, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(M2->right_pin_port, M2->right_pin, GPIO_PIN_SET);
	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,300);	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,300);
}

void Motor_Turn_Left(Motor_t *M1, Motor_t *M2)
{
//    HAL_GPIO_WritePin(M1->left_pin_port, M1->left_pin, GPIO_PIN_SET);
//    HAL_GPIO_WritePin(M1->right_pin_port, M1->right_pin, GPIO_PIN_RESET);

//     HAL_GPIO_WritePin(M2->left_pin_port, M2->left_pin, GPIO_PIN_SET);
//    HAL_GPIO_WritePin(M2->right_pin_port, M2->right_pin, GPIO_PIN_RESET);
	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,300);	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,0);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,300);
}

void Motor_Turn_Right(Motor_t *M1, Motor_t *M2)
{
//    HAL_GPIO_WritePin(M1->left_pin_port, M1->left_pin, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(M1->right_pin_port, M1->right_pin, GPIO_PIN_SET);

//     HAL_GPIO_WritePin(M2->left_pin_port, M2->left_pin, GPIO_PIN_RESET);
//    HAL_GPIO_WritePin(M2->right_pin_port, M2->right_pin, GPIO_PIN_SET);
	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,300);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,0);	
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,300);
	__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,0);
	
}

